#include "ColorTracker.h"
#include "../utilities/ObjPosition.h"

#include <vector>
#include <string.h>

using namespace std;


ObjPosition obj_pos = ObjPosition(3*BIN_SIZE + CENTER_BIN_WIDTH,0,10.0);


ColorTracker::ColorTracker(Driver* driver):m_bins({0.0}) {

	ROS_INFO("In ColorTracker::ColorTracker");
	// set driver
	m_driver = driver;

	m_loop_rate = new ros::Rate(10);

	// subscribe to vision system
	m_subscriber = m_node_handler.subscribe("/VisionNode/ObjPosition", 1000, &ColorTracker::positionSetter, this);

	// force connection establishment
	m_loop_rate->sleep();
	sleep(1);

	// construct bin mapper
	for (int i = 0 ; i < BIN_SIZE ; ++i) {
		
		m_bins[i] = 0.75;
		m_bins[i+BIN_SIZE] = 0.5;
		m_bins[i+2*BIN_SIZE] = 0.25;
		
		m_bins[i+3*BIN_SIZE + CENTER_BIN_WIDTH] = -0.25;
		m_bins[i+4*BIN_SIZE + CENTER_BIN_WIDTH] = -0.5;
		m_bins[i+5*BIN_SIZE + CENTER_BIN_WIDTH] = -0.75;
	}
}

ColorTracker::~ColorTracker() {

	// stop robot
	m_driver->stop();
}

bool ColorTracker::track() {

	ROS_INFO("ColorTracker::track");

	double rotateAngle = 0.0;
	double speed = 0.0;

//	TODO CHECK IF CAN delete
	m_loop_rate = new ros::Rate(10);

	ObjPosition cur_pos(0,0,0.0);

	while (ros::ok()) {

		ros::spinOnce();	

		cur_pos = safeRead();

		ROS_INFO("cur_pos.m_x = %d, cur_pos.m_y = %d, cur_pos.m_dist = %f",cur_pos.m_x,cur_pos.m_y,cur_pos.m_dist);

		// check if object is reachable
		if (obj_pos.m_dist >= MAX_DIST_TO_OBJ) {

			ROS_ERROR("IN STOP IF");
			// stop robot
			m_driver->stop();

			// done task
			return true;
		}

		// calculate rotation angle according to horizontal position
		rotateAngle = calcHorizontalOrientation(obj_pos.m_x);

		// calculate speed proportional to distance to object
		speed = 0.2;//calcSpeed(obj_pos.m_dist, TIME_TO_REACH_OBJ);

		// move towards object
		m_driver->move(speed, rotateAngle);
//		ROS_ERROR("AFTER m_driver->move(%f, %f);",speed,rotateAngle);
	}

	return false;
}

double ColorTracker::calcSpeed(double distToObj, double timeToMove) {
	return distToObj / timeToMove;
}

double ColorTracker::calcHorizontalOrientation(int xCord) {

	// return orientation fixing to bin matching horizontal position
	if (xCord >=0 && xCord < IMAGE_WIDTH){

		return m_bins[xCord];
	}

	return m_bins[IMAGE_WIDTH/2 + 1];
}

ObjPosition ColorTracker::safeRead() {


	std::unique_lock<std::mutex> mlock(mutex_);
	ObjPosition cur_pos = obj_pos;
	mlock.unlock();

	return cur_pos;
}


void ColorTracker::positionSetter(const std_msgs::String::ConstPtr& msg) { // callback

	// check error in message
	if (msg->data.empty()) {
		ROS_ERROR("ColorTracker::positionSetter- EMPTY MESSAGE");
		return;
	}

	// parse values out of message
	int x,y;
	float dist;
	parseMsg(msg->data,x,y,dist);


	ROS_INFO("ColorTracker::positionSetter- x = %d, y = %d, dist = %f",x, y ,dist); // TODO DEBUG PRINT!!!

	// set values of object position
	std::unique_lock<std::mutex> mlock(mutex_);
	obj_pos.m_x = x;
	obj_pos.m_y = y;
	obj_pos.m_dist = dist;
	mlock.unlock();
}

void ColorTracker::parseMsg(string msg, int& x, int& y, float& dist) {

	// split message by delimiter
	char dataChars[msg.length()];
	strcpy(dataChars, msg.c_str());

	// store tokens
	vector<string> tokens;
	char* pch = strtok(dataChars,DELIMITER);
	while (pch != NULL) {
		tokens.push_back(pch);
		pch = strtok(NULL, DELIMITER);
	}

	// extract data
	x = atoi(tokens[0].c_str());
	y = atoi(tokens[1].c_str());
	dist = atof(tokens[2].c_str());
}
